//Spam Spam Spam Spam
//Spam Spam Spam Spam
//Spam Spam Spam Spam
//Spam Spam Spam Spam
//Spam Spam Spam Spam
//Spam Spam Spam Spam
//Lovely Spam Lovely Spam
//Lovely Spam Lovely Spam
//Lovely Spam Lovely Spam
//Lovely Spam Wondeful Spam
//Lovely Spam Wondeful Spam
//Lovely Spam Wondeful Spam
package edu.wpi.first.wpilibj.templates;
import com.sun.squawk.microedition.io.FileConnection;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.DriverStationLCD;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Timer;
import java.io.IOException;
import java.io.OutputStream;
import java.io.PrintStream;
import javax.microedition.io.Connector;
public class Hi extends IterativeRobot {
    DigitalInput LEFT, RIGHT, MID;
    RobotDrive weeeee;
    double DIRECTION; //I'm assuming I can't get my initial direction..
    Jaguar LeftJag, RightJag;
    double sweep = .3;
    double cycle = 0;
    double maxturntime = 25;
    boolean isOnLine = false;
    int AutonMode = 0;
    boolean endOfLine = false;
    boolean hasReachedFork = false;
    boolean prevL = false, prevM = false, prevR = false;
    boolean prevButton = false;
    double swept = .3;
    //boolean Swept = false;
    //boolean[] previousState = new boolean[3];
    String Status = "Initializing...";
    private Joystick RobotControl;
    double TurnGain = .2;
    double Speeding = .5;
    boolean previous4 = false, previous3 = false, previous5 = false, previous2 = false, previous6 = false, previous7 = false;
    private int definitiveness = 5;
    int StopRightHereYouAreReallyAtTheEnd = 0;
    private int BatteryCounter = 0;
    private int GameCounter = 0;
    private FileConnection fc;
    private OutputStream File;
    private PrintStream Print;
    private double prevX;
    private double prevY;
    private double prevLJag;
    private double prevRJag;
    public void robotInit() {
        int Counter;
        DIRECTION = 0;
        Counter = 0;
        while (LeftJag == null)
        {
            Counter +=1;
            try
            {
                LeftJag = new Jaguar(1);//Front Left
            }catch (Exception e)
            {
                printException("Jag1 failed to initialize",e);
                if (Counter > 3)
                {
                break;
                }
            }
        }
        Counter = 0;
        while (RightJag == null)
        {
            Counter +=1;
            try
            {
                RightJag = new Jaguar(2);//Back Left
            }catch (Exception e)
            {
                printException("Jag2 failed to initialize",e);
                if (Counter > 3)
                {
                break;
                }
            }
        }
        Counter = 0;
        while (LEFT == null)
        {
            Counter +=1;
            try
            {
                LEFT = new DigitalInput(1);
            }catch (Exception e)
            {
                printException("Left Photosensor failed to initilaize",e);
                if (Counter > 3)
                {
                break;
                }
            }
        }
        Counter = 0;
        while (MID == null)
        {
            Counter += 1;
            try
            {
                MID = new DigitalInput(2);
            }catch (Exception e)
            {
                printException("Center Photosensor failed to initiliaize",e);
                if (Counter > 3)
                {
                break;
                }
            }
        }
        Counter = 0;
        while (RIGHT == null)
        {
            Counter += 1;
            try
            {
                RIGHT = new DigitalInput(3);
            } catch(Exception e)
            {
                printException("Right Photosensor failed to initilaiwerwer",e);
                if (Counter > 3)
                {
                break;
                }
            }
        }
        Counter = 0;
        while (weeeee==null)
        {
            Counter += 1;
            try
            {
                weeeee = new RobotDrive(LeftJag,RightJag);//Not completely sure... Ask someone... MmmmmK?
        }catch(Exception e)
            {
                 printException("Failed to start RobotDrive",e);
                 if (Counter > 3)
                {
                break;
                }
            }
        }
        Counter = 0;
        while (RobotControl==null)
        {
            Counter += 1;
            try
            {
                RobotControl = new Joystick(1);//Not completely sure... Ask someone... MmmmmK?
        }catch(Exception e)
            {
                 printException("Failed to start Joystick",e);
                 if (Counter > 3)
                {
                break;
                }
            }
        }
        DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser2, 1,Status);
        DriverStationLCD.getInstance().updateLCD();
        try {
            fc = (FileConnection) Connector.open("file:///output.csv", Connector.WRITE);
            fc.create();
            File = fc.openOutputStream();
        }
        catch (IOException ex) {
            ex.printStackTrace();
        }
       Print = new PrintStream(File);
        Print.println("Time:,Battery:,Joystick:,Jaguars:,Sensors:");
    }
        public void disabledInit()
    {
        //Stop moving
        weeeee.arcadeDrive(0,0);
        hasReachedFork = false;
        isOnLine = false;
        endOfLine = false;
        StopRightHereYouAreReallyAtTheEnd = 0;
        BatteryCounter = 0;
GameCounter = 0;

    }
    public void disabledPeriodic()
    {
        if (!RobotControl.getRawButton(2) && previous2)
        {
            Speeding = Math.max(Speeding-.05,0);
        }
        if (!RobotControl.getRawButton(3) && previous3)
        {
            Speeding = Math.min(Speeding+.05,1);
        }

        if (!RobotControl.getRawButton(4) && previous4)
        {
            TurnGain = Math.max(TurnGain-.05,0);
        }
        if (!RobotControl.getRawButton(5) && previous5)
        {
            TurnGain = Math.min(TurnGain+.05,1);
        }

        if (!RobotControl.getRawButton(6) && previous6)
        {
           definitiveness += 1;
        }
        if (!RobotControl.getRawButton(7) && previous7)
        {
            definitiveness -= 1;
        }

        if (!RobotControl.getRawButton(1) && prevButton)
        {
        AutonMode += 1;
        if (AutonMode > 2)
        {
            AutonMode = 0;
        }
        }
        switch (AutonMode)
        {
            case 0:
        DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser2, 1,"Straight Line");
        DriverStationLCD.getInstance().updateLCD();
            break;
            case 1:
        DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser2, 1,"Fork Left~~~~~~~~");
        DriverStationLCD.getInstance().updateLCD();
            break;
            case 2:
        DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser2, 1,"Fork Right~~~~~~~~");
        DriverStationLCD.getInstance().updateLCD();
            break;
            default:
        DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser2, 1,"I'm Confused...");
        DriverStationLCD.getInstance().updateLCD();
            break;
        }
        DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser3, 1,"Speed: "+Speeding+"        ");
        DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser4, 1,"Turn: "+TurnGain+"        ");
        DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser5, 1,"StopAfter: "+definitiveness+"        ");
        DriverStationLCD.getInstance().updateLCD();
        prevButton = RobotControl.getRawButton(1);
         previous2 = RobotControl.getRawButton(2);
          previous3 = RobotControl.getRawButton(3);
           previous4 = RobotControl.getRawButton(4);
            previous5 = RobotControl.getRawButton(5);
             previous6 = RobotControl.getRawButton(6);
              previous7 = RobotControl.getRawButton(7);
    }
     public void teleopPeriodic()
    {

        Status = "TELEOP CONTROL ";
        if (RobotControl.getRawButton(1))
        {
            Status = "Firin' the laser!";
            //weeeee.arcadeDrive(0,1);//WEEE!
        }

        weeeee.arcadeDrive(RobotControl);
        boolean left = !LEFT.get();
        boolean mid = !MID.get();
        boolean right = !RIGHT.get();
        Status = ""+left + "-" + mid + "-" + right;
        DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser2, 1,Status);
        DriverStationLCD.getInstance().updateLCD();
         String WhatIsToBeInserted, Batt, Joy, Jag;
   WhatIsToBeInserted = "HI!";

              //Counter += 1;
              if (GameCounter <= 6000)
              {
GameCounter += 1;
              BatteryCounter += 1;
                Batt = "NULL";//BatteryCounter >= 100? ""+Battery.getValue():"";
                if (BatteryCounter >= 100)
                {
                    BatteryCounter=0;
                }
                Joy = (RobotControl.getX() != prevX || RobotControl.getY() != prevY)? ""+RobotControl.getX()+":"+RobotControl.getY():"";
                Jag = (LeftJag.get() != prevLJag || RightJag.get() != prevRJag)? ""+LeftJag.get()+":"+RightJag.get():"";
                WhatIsToBeInserted = ""+GameCounter+","+Batt+","+Joy+","+Jag;

              // if ((prevL != LeftSensor.get()) || (prevR != RightSensor.get()) || (prevM != MidSensor.get()))
                 //           {
                            String leftMG, rightMG, midMG;
                            leftMG = "Left="+!LEFT.get();
                            midMG = "  Mid="+!MID.get();
                            rightMG =  "  Right="+!RIGHT.get();
                            WhatIsToBeInserted+=","+leftMG+midMG+rightMG;
                   //         }

                prevX = RobotControl.getX();
                prevY = RobotControl.getY();
                prevRJag = RightJag.get();
                prevLJag = LeftJag.get();
             Print.println(WhatIsToBeInserted);
            Print.flush();
            
    }
    }
    public void autonomousPeriodic() {
         boolean left, right, mid;
        //boolean left, right, mid;
        //left = !LEFT.get(); mid = !MID.get(); right = !RIGHT.get();
       /*     if (!left&&!mid&&!right)
            {
                endOfLine = true;
                isOnLine = false;
                weeeee.arcadeDrive(0,0);
           } */
        switch (AutonMode)
        {
            case 0://Straight Line
       
        left = !LEFT.get(); mid = !MID.get(); right = !RIGHT.get();
        Status = ""+left + "-" + mid + "-" + right;
        if (left&&mid&&right)
        {
            endOfLine = true;
            weeeee.arcadeDrive(0,0);
            //I do know shun cakes!
            return;
        }
        if (!endOfLine)
        {
        if (!isOnLine)
        {
        findLineAndThenDrive(Speeding);
        }
        else
        {
            swept = .6;
        alignToLinesAndDriveAndStuff(Speeding,TurnGain);
       
        }
        prevL = left; prevM = mid; prevR = right;
        /*previousState[0] = left;
        previousState[1] = mid;
        previousState[2] = right;*/
        //DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser3, 1,"LEFT="+Math.floor(left)+" RIGHT="+right+" MID="+mid);
        DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser2, 1,Status);
        DriverStationLCD.getInstance().updateLCD();
         }
        else
        {
            //weeeee.arcadeDrive(0,0);
            DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser2, 1,"DONE!");
        DriverStationLCD.getInstance().updateLCD();
        }
        break;

        
            case 1://Fork Left
        left = !LEFT.get(); mid = !MID.get(); right = !RIGHT.get();
        if (!endOfLine)
        {
        if (!isOnLine)
        {
        findLineAndThenDrive(Speeding);
            try {
                Status = "Sweeping...";
                }
            catch (Exception ex) {

            }
        }
        else
        {
        forkLeft(Speeding,TurnGain);
        Status = "Following...";
        }
        prevL = left; prevM = mid; prevR = right;
        /*previousState[0] = left;
        previousState[1] = mid;
        previousState[2] = right;*/
        //DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser3, 1,"LEFT="+Math.floor(left)+" RIGHT="+right+" MID="+mid);
        DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser2, 1,Status);
        DriverStationLCD.getInstance().updateLCD();
        }
        break;
        case 2://Fork Right
        left = !LEFT.get(); mid = !MID.get(); right = !RIGHT.get();
        if (!endOfLine)
        {
        if (!isOnLine)
        {
        findLineAndThenDrive(Speeding);
            try {
                Status = "Sweeping...";
                }
            catch (Exception ex) {

            }
        }
        else
        {
        forkRight(Speeding,TurnGain);
        Status = "Following...";
        }
        prevL = left; prevM = mid; prevR = right;
        /*previousState[0] = left;
        previousState[1] = mid;
        previousState[2] = right;*/
        //DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser3, 1,"LEFT="+Math.floor(left)+" RIGHT="+right+" MID="+mid);
        DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser2, 1,Status);
        DriverStationLCD.getInstance().updateLCD();
        }
        break;
        }
        
    }


    public void printException(String error,Exception e)
    {
        Status += error+" ::: ";
        e.printStackTrace();
        Timer.delay(3);
    }
    public void findLineAndThenDrive(double speed)
    {
        if (sweep > 0)
        {
        weeeee.arcadeDrive(speed,swept);
        }
        else
        {
            weeeee.arcadeDrive(speed,-swept);
        }
        cycle+=1;
        if (cycle > maxturntime)
        {
            cycle = 0;
            sweep *= -1;
            if (sweep < 0)
            {
            maxturntime+=10;
            }
        }
        if ((!MID.get())||(!LEFT.get())||(!RIGHT.get()))
        {
            isOnLine = true;
            return;
        }
    }
    public void alignToLinesAndDriveAndStuff(double speed,double inc)
    {
        boolean left, right, mid;
        left = !LEFT.get(); mid = !MID.get(); right = !RIGHT.get();
        
        if (mid)//Don't Panic, we are on the line!
        {
            if (left && !right)//If we are leaning towards the left...
            {
                DIRECTION+=inc;
                //speed /= 2;
            }
            if (!left && right)//If we are leaning towards the right...
            {
                DIRECTION-=inc;
                //speed /= 2;
                
            }
            if (left && right)//If we are at the end...
            {
                //I'll need to know our direction...
            }
        }
        else
        {
            if (left && !right)//Leaning towards the left a lot?
            {
                DIRECTION+=inc;
                //speed /= 2;
            
            }
            if (!left && right)//Leaning towards the right a lot?
            {
                DIRECTION-=inc;
              //speed /= 2;
            }
            if (left && right)//We are at the fork!
            {
                //I'll need to decide... for now, lets go left
                DIRECTION+=inc;
              //speed /= 2;
            }
            if (!left && !right)//We are completely lost...
            {
                //isOnLine = false;
                //return;
            }
        }
        
        weeeee.arcadeDrive(speed, DIRECTION);
        DIRECTION = 0;
    }

         public void forkLeft(double speed,double inc)
    {
        boolean left, right, mid;
        left = !LEFT.get(); mid = !MID.get(); right = !RIGHT.get();

        if (mid)//Don't Panic, we are on the line!
        {
            if (left && !right)//If we are leaning towards the left...
            {
                DIRECTION+=inc;
                //speed /= 2;
                if (prevL&&prevR&&prevM)
                {
                    hasReachedFork = true;
                }
            }
            if (!left && right)//If we are leaning towards the right...
            {
                DIRECTION-=inc;
                //speed /= 2;
            if (prevL&&prevR&&prevM)
                {
                    hasReachedFork = true;
                }
            }
            if (left && right)//If we are at the end...
            {
                //I'll need to know our direction...
                if (hasReachedFork)
                {
                    StopRightHereYouAreReallyAtTheEnd += 1;
                    if (StopRightHereYouAreReallyAtTheEnd >= definitiveness)
                    {
                    endOfLine = true;
                    weeeee.arcadeDrive(0,0);
                    return;
                    }
                }
                else
                {
                    DIRECTION+=inc;
                }
            }
        }
        else
        {
            if (left && !right)//Leaning towards the left a lot?
            {
                DIRECTION+=inc;
                //speed /= 2;
                if (prevL&&prevR&&prevM)
                {
                    
                    hasReachedFork = true;
                    DIRECTION += inc*2;
                }
            }
            if (!left && right)//Leaning towards the right a lot?
            {
                DIRECTION-=inc;
              //speed /= 2;
                if (prevL&&prevR&&prevM)
                {
                    hasReachedFork = true;
                    DIRECTION += inc*2;
                }
            }
            if (left && right)//We are at the fork!
            {
                //I'll need to decide... for now, lets go left
                DIRECTION+=inc;
              //speed /= 2;
            }
            if (!left && !right)//We are completely lost...
            {
                //isOnLine = false;
                return;
            }
        }

        weeeee.arcadeDrive(speed, DIRECTION);
        DIRECTION = 0;
    }
     public void forkRight(double speed,double inc)
    {
        boolean left, right, mid;
        left = !LEFT.get(); mid = !MID.get(); right = !RIGHT.get();

        if (mid)//Don't Panic, we are on the line!
        {
            if (left && !right)//If we are leaning towards the left...
            {
                DIRECTION+=inc;
                //speed /= 2;
                if (prevL&&prevR&&prevM)
                {
                    hasReachedFork = true;
                }
            }
            if (!left && right)//If we are leaning towards the right...
            {
                DIRECTION-=inc;
                //speed /= 2;
            if (prevL&&prevR&&prevM)
                {
                    hasReachedFork = true;
                }
            }
            if (left && right)//If we are at the end...
            {
                //I'll need to know our direction...
                if (hasReachedFork)
                {
                    StopRightHereYouAreReallyAtTheEnd += 1;
                    if (StopRightHereYouAreReallyAtTheEnd >= definitiveness)
                    {
                    endOfLine = true;
                    weeeee.arcadeDrive(0,0);
                    return;
                    }
                }
                else
                {
                    DIRECTION-=inc;
                }
            }
        }
        else
        {
            if (left && !right)//Leaning towards the left a lot?
            {
                DIRECTION+=inc;
                //speed /= 2;
                if (prevL&&prevR&&prevM)
                {
                    hasReachedFork = true;
                    DIRECTION -= inc*2;
                }
            }
            if (!left && right)//Leaning towards the right a lot?
            {
                DIRECTION-=inc;
              //speed /= 2;
                if (prevL&&prevR&&prevM)
                {
                    hasReachedFork = true;
                    DIRECTION -= inc*2;
                }
            }
            if (left && right)//We are at the fork!
            {
                //I'll need to decide... for now, lets go left
                DIRECTION-=inc;
              //speed /= 2;
            }
            if (!left && !right)//We are completely lost...
            {
                //isOnLine = false;
                return;
            }
        }

        weeeee.arcadeDrive(speed, DIRECTION);
        DIRECTION = 0;
    }
}